//增加  
#include "SCS.h"

#include <string>
#include <stdio.h>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Int8.h>
#include <tf/transform_broadcaster.h>
#include <sensor_msgs/JointState.h>
#include <unistd.h>
#include <iostream>

// 位置  0:张开  1:闭合  2:初始化位置
// scs.WriteSpeAPos(0,2);                                //同时写舵机位置
// scs.WritePos(2);                                              //写舵机和速度，参数含义参照协议手册
// scs.Read_Cur_Pos();                                       //获取夹爪当前位置
// scs.Read_Cur_Load();                                     //获取夹爪当前负载


void gripper_control_callback(const std_msgs::Int8ConstPtr &msg)
{
	SCS scs;
    ROS_INFO("gripper control: %d", msg->data);
    scs.WritePos(msg->data);
}

int main(int argc, char** argv)
{	
	ros::init(argc, argv, "innfos_write_node");
    ros::NodeHandle nh;
	ros::Subscriber sub0 = nh.subscribe("gripper_control", 1, gripper_control_callback);
	ros::spin();
}